import numpy as np
import open3d as o3d


class BoneStorageModel:
    """
    骨仓建模类 (Bone Storage Model)
    用于创建骨仓的三维模型
    """

    def __init__(self, width, depth, height,rotation_matrix):
        """
        初始化骨仓模型

        参数:
            length (float): 骨仓的长度
            width (float): 骨仓的宽度
            height (float): 骨仓的高度
        """
        self.width = width
        self.depth = depth
        self.height = height
        self.rotation_matrix = rotation_matrix

    def create_wireframe(self, color, rotation_matrix):
        lines = [
            [0, 1], [1, 2], [2, 3], [3, 0],
            [4, 5], [5, 6], [6, 7], [7, 4],
            [0, 4], [1, 5], [2, 6], [3, 7]
        ]
        points = [
            [-self.width / 2, -self.depth / 2, -self.height / 2], [self.width / 2, -self.depth / 2, -self.height / 2],
            [self.width / 2, self.depth / 2, -self.height / 2], [-self.width / 2, self.depth / 2, -self.height / 2],
            [-self.width / 2, -self.depth / 2, self.height / 2], [self.width / 2, -self.depth / 2, self.height / 2],
            [self.width / 2, self.depth / 2, self.height / 2], [-self.width / 2, self.depth / 2, self.height / 2]
        ]

        line_set = o3d.geometry.LineSet()
        line_set.points = o3d.utility.Vector3dVector(points)
        line_set.lines = o3d.utility.Vector2iVector(lines)
        line_set.paint_uniform_color(color)

        if rotation_matrix is not None:
            if rotation_matrix.shape == (3, 3):
                rotation_matrix = np.vstack([rotation_matrix, [0, 0, 0]])
                rotation_matrix = np.hstack([rotation_matrix, [[0], [0], [0], [1]]])
            line_set.transform(rotation_matrix)

        return line_set
    #todo 在以相机为中心的长方体框中再建模料仓，相机位置应该在料仓外部